package ar.edu.untref.robotica.motorfisico;


public class Calculadora {

	// /**
	// *
	// * @param velRuedaIzquierda
	// * @param velRuedaDerecha
	// * @return cambio de orientaci�n en grados
	// */
	// public static Double getCambioDeOrientacion(Double velRuedaIzquierda,
	// Double velRuedaDerecha) {
	//
	// Double desplazamientoLineal = getDesplazamientoLineal();
	// Double distanciaDesplazamientoIzquierda = getDistanciaDesplazamiento(
	// desplazamientoLineal, velRuedaIzquierda);
	// Double distanciaDesplazamientoDerecha = getDistanciaDesplazamiento(
	// desplazamientoLineal, velRuedaDerecha);
	//
	// return Math
	// .toDegrees((distanciaDesplazamientoDerecha -
	// distanciaDesplazamientoIzquierda)
	// / Configuracion.instancia().getDistanciaEntreRuedas());
	// }
	//
	// private static Double getDesplazamientoLineal() {
	//
	// double diametroRuedas = Configuracion.instancia().getDiametroRuedas();
	// double reduccionMotorRueda = Configuracion.instancia()
	// .getReduccionRuedaMotor();
	// double pulsosEncoder = Configuracion.instancia().getPulsosEncoder();
	//
	// Double desplazamientoLineal = (Math.PI * diametroRuedas)
	// / (reduccionMotorRueda * pulsosEncoder);
	//
	// return desplazamientoLineal;
	// }
	//
	// private static Double getDistanciaDesplazamiento(
	// Double desplazamientoLineal, Double velocidadRueda) {
	//
	// return desplazamientoLineal * velocidadRueda;
	// }

	public static double getVelocidadDesplazamiento(
			double velocidadRuedaIzquierda, double velocidadRuedaDerecha) {
		return (velocidadRuedaDerecha + velocidadRuedaIzquierda) / 2;
	}

	public static double getCambioAngulo(Rueda ruedaIzquierda,
			Rueda ruedaDerecha) {

		double radio = ruedaIzquierda.getDiametroRueda() / 2;

		// -1 porque si no me lo da al revez el angulo. supongo que se puede
		// hacer ruedaIz-ruedaDer, ver!
		return -1
				* radio
				* (revolucionesRueda(ruedaDerecha) - revolucionesRueda(ruedaIzquierda));
	}

	/**
	 * 
	 * @param rueda
	 * @return revoluciones de la rueda en GRADOS
	 */
	private static double revolucionesRueda(Rueda rueda) {

		double radio = rueda.getDiametroRueda() / 2;

		return (rueda.getVelocidad().doubleValue() * 360) / (2 * Math.PI * radio);
	}

}
